{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "isam_intro_md"
      },
      "source": [
        "# ISAM"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "isam_desc_md"
      },
      "source": [
        "`gtsam.ISAM` (Incremental Smoothing and Mapping) is a class that inherits from `BayesTree` and adds an `update` method. This method allows for efficient incremental updates to the solution when new factors (e.g., new measurements) are added to the problem.\n",
        "\n",
        "Instead of re-eliminating the entire factor graph from scratch, iSAM identifies the part of the Bayes tree affected by the new factors, removes that part, and re-eliminates only the necessary variables, merging the results back into the existing tree.\n",
        "\n",
        "Like `BayesTree`, it's templated (e.g., `GaussianISAM` which inherits from `GaussianBayesTree`). For practical applications requiring incremental updates, `ISAM2` is often preferred due to further optimizations like fluid relinearization and support for variable removal, but `ISAM` demonstrates the core incremental update concept based on the Bayes tree."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "isam_colab_md"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/inference/doc/ISAM.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "isam_pip_code",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "%pip install --quiet gtsam-develop"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "id": "isam_import_code"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "\n",
        "# Use Gaussian variants for demonstration\n",
        "from gtsam import GaussianFactorGraph, Ordering, GaussianISAM, GaussianBayesTree\n",
        "from gtsam import symbol_shorthand\n",
        "\n",
        "X = symbol_shorthand.X\n",
        "L = symbol_shorthand.L"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "isam_create_md"
      },
      "source": [
        "## Initialization\n",
        "\n",
        "An `ISAM` object can be created empty or initialized from an existing `BayesTree`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "isam_init_code",
        "outputId": "89abcdef-0123-4567-89ab-cdef01234567"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Initial BayesTree:\n",
            ": cliques: 1, variables: 1\n",
            "- p(x0)\n",
            "  R = [ 1 ]\n",
            "  d = [ 0 ]\n",
            "  mean: 1 elements\n",
            "  x0: 0\n",
            "  logNormalizationConstant: -0.918939\n",
            "  No noise model\n",
            "ISAM from BayesTree:\n",
            "GaussianISAM: : cliques: 1, variables: 1\n",
            "GaussianISAM: - p(x0)\n",
            "  R = [ 1 ]\n",
            "  d = [ 0 ]\n",
            "  mean: 1 elements\n",
            "  x0: 0\n",
            "  logNormalizationConstant: -0.918939\n",
            "  No noise model\n"
          ]
        }
      ],
      "source": [
        "# Create an empty ISAM object\n",
        "isam1 = GaussianISAM()\n",
        "\n",
        "# Create from an existing Bayes Tree (e.g., from an initial batch solve)\n",
        "initial_graph = GaussianFactorGraph()\n",
        "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n",
        "initial_graph.add(X(0), -np.eye(1), np.zeros(1), model) # Prior on x0\n",
        "\n",
        "initial_bayes_tree = initial_graph.eliminateMultifrontal(Ordering([X(0)]))\n",
        "print(\"Initial BayesTree:\")\n",
        "initial_bayes_tree.print()\n",
        "\n",
        "isam2 = GaussianISAM(initial_bayes_tree)\n",
        "print(\"ISAM from BayesTree:\")\n",
        "isam2.print()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "isam_update_md"
      },
      "source": [
        "## Incremental Update\n",
        "\n",
        "The core functionality is the `update(newFactors)` method."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "isam_update_code",
        "outputId": "9abcdef0-1234-5678-9abc-def012345678"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "ISAM after first update (x0, x1):\n",
            "GaussianISAM: : cliques: 1, variables: 2\n",
            "GaussianISAM: - p(x1 x0 )\n",
            "  R = [  1 -1 ]\n",
            "      [  0  1 ]\n",
            "  d = [ 0 0 ]\n",
            "  mean: 2 elements\n",
            "  x0: 0\n",
            "  x1: 0\n",
            "  logNormalizationConstant: -1.83788\n",
            "  No noise model\n",
            "\n",
            "ISAM after second update (x0, x1, x2):\n",
            "GaussianISAM: : cliques: 2, variables: 3\n",
            "GaussianISAM: - p(x0 x1 )\n",
            "  R = [   1.41421 -0.707107 ]\n",
            "      [         0  0.707107 ]\n",
            "  d = [ 0 0 ]\n",
            "  mean: 2 elements\n",
            "  x0: 0\n",
            "  x1: 0\n",
            "  logNormalizationConstant: -1.83788\n",
            "  No noise model\n",
            "GaussianISAM: | - p(x2 | x1)\n",
            "  R = [ 1 ]\n",
            "  S[x1] = [ -1 ]\n",
            "  d = [ 0 ]\n",
            "  logNormalizationConstant: -0.918939\n",
            "  No noise model\n"
          ]
        }
      ],
      "source": [
        "# Start with the ISAM object containing the prior on x0\n",
        "isam = GaussianISAM(initial_bayes_tree)\n",
        "model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0)\n",
        "\n",
        "# --- First Update ---\n",
        "new_factors1 = GaussianFactorGraph()\n",
        "new_factors1.add(X(0), -np.eye(1), X(1), np.eye(1), np.zeros(1), model) # x0 -> x1\n",
        "isam.update(new_factors1)\n",
        "\n",
        "print(\"ISAM after first update (x0, x1):\")\n",
        "isam.print()\n",
        "\n",
        "# --- Second Update ---\n",
        "new_factors2 = GaussianFactorGraph()\n",
        "new_factors2.add(X(1), -np.eye(1), X(2), np.eye(1), np.zeros(1), model) # x1 -> x2\n",
        "isam.update(new_factors2)\n",
        "\n",
        "print(\"\\nISAM after second update (x0, x1, x2):\")\n",
        "isam.print()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "isam_solve_md"
      },
      "source": [
        "## Solution and Marginals\n",
        "\n",
        "Since `ISAM` inherits from `BayesTree`, you can use the same methods like `optimize()` and `marginalFactor()` after performing updates."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "isam_solve_code",
        "outputId": "abcdef01-2345-6789-abcd-ef0123456789"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Optimized Solution after updates:\n",
            "VectorValues: 3 elements\n",
            "  x0: 0\n",
            "  x1: 0\n",
            "  x2: 0\n"
          ]
        }
      ],
      "source": [
        "# Get the solution from the final ISAM state\n",
        "solution = isam.optimize()\n",
        "print(\"Optimized Solution after updates:\")\n",
        "solution.print()"
      ]
    }
  ],
  "metadata": {
    "colab": {
      "provenance": []
    },
    "kernelspec": {
      "display_name": "gtsam",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.13.1"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}
